#此方案用于云台与摄像头固定方案。且直接识别红色靶心。
import cv2
import numpy as np
import serial
import time
import threading
from collections import deque

class TargetingSystem:
    def __init__(self):
        # 系统参数配置
        self.camera_index = 1
        self.target_color = "red"  # 靶心颜色
        #self.serial_port = "/dev/ttyUSB0"  # STM32串口
        #self.baud_rate = 115200
        self.pid_params = {'kp': 0.3, 'ki': 0.01, 'kd': 0.1}  # PID参数
        
        # 系统状态
        self.running = False
        self.calibrated = False
        self.target_found = False
        self.laser_on = False
        
        # 图像处理参数
        self.target_radius = 10  # 靶心半径(pixels)
        self.calibration_data = {
            'camera_matrix': None,
            'dist_coeffs': None,
            'laser_offset': (0, 0)  # 激光与摄像头中心的偏移
        }
        
        # PID控制变量
        self.pid_x = PIDController(self.pid_params)
        self.pid_y = PIDController(self.pid_params)
        self.error_history = deque(maxlen=10)
        
        # 初始化系统
        self.init_camera()
        self.init_serial()
        self.calibrate_system()
    
    def init_camera(self):
        """初始化摄像头"""
        self.cap = cv2.VideoCapture(self.camera_index)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        self.cap.set(cv2.CAP_PROP_FPS, 30)
        
        if not self.cap.isOpened():
            raise RuntimeError("无法打开摄像头")
    
    def init_serial(self):
        """初始化串口通信"""
        try:
            #self.ser = serial.Serial(self.serial_port, self.baud_rate, timeout=1)
            time.sleep(2)  # 等待串口初始化
        except serial.SerialException:
            raise RuntimeError("无法打开串口，请检查连接")
    
    def calibrate_system(self):
        """系统标定：确定激光与摄像头中心的偏移"""
        # 在实际应用中，这里应使用标定板进行精确标定
        # 为简化，我们假设激光与摄像头中心对齐
        self.calibration_data['laser_offset'] = (0, 0)
        self.calibrated = True
        print("系统标定完成")
    
    def detect_target(self, frame):
        gray_img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        blurred = cv2.GaussianBlur(gray_img, (5, 5), 0)
        edges = cv2.Canny(blurred, 50, 150)
        _,contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        max_area = 0
        best_rect = None
        x, y, w, h = None, None, None, None
        center_x,center_y=None,None
        for contour in contours:
            area = cv2.contourArea(contour)
            # 过滤小区域
            if area < 2000:
                continue
                
            # 近似轮廓为多边形
            epsilon = 0.02 * cv2.arcLength(contour, True)
            approx = cv2.approxPolyDP(contour, epsilon, True)
            
            # 检查是否为四边形（矩形）
            if len(approx) == 4:
                rect_x, rect_y, rect_w, rect_h = cv2.boundingRect(approx)
                aspect_ratio = float(rect_w) / rect_h
                
                # 过滤过于狭长的形状
                if 0.5 < aspect_ratio < 2.0 and area > max_area:
                    max_area = area
                    best_rect = approx
                    x, y, w, h = rect_x, rect_y, rect_w, rect_h
                    center_x=x+w/2.0
                    center_y=y+h/2.0
                    self.target_found = True
                    return best_rect, center_x,center_y
        
        self.target_found = False
        return None, 0, 0
    
    

  
            
    def calculate_error(self, target_pos):
        """计算目标位置与图像中心的误差"""
        height, width, _ = self.current_frame.shape
        center_x, center_y = width // 2, height // 2
        
        # 应用激光偏移校准
        target_x, target_y = target_pos
        target_x += self.calibration_data['laser_offset'][0]
        target_y += self.calibration_data['laser_offset'][1]
        
        error_x = target_x - center_x
        error_y = target_y - center_y
        
        # 记录误差历史
        self.error_history.append((error_x, error_y))
        
        return error_x, error_y
    
    def pid_control(self, error_x, error_y):
        """PID控制计算"""
        control_x = self.pid_x.update(error_x)
        control_y = self.pid_y.update(error_y)
        return control_x, control_y
    
    def send_control_command(self, control_x, control_y):
        """发送控制指令给下位机"""
        # 将控制量转换为云台角度指令
        # 实际应用中需要根据云台参数进行转换
        
        # 构建指令字符串 "X:value,Y:value\n"
        command = f"X:{control_x:.2f},Y:{control_y:.2f}\n"
        
        #try:
            #self.ser.write(command.encode('utf-8'))
        #except serial.SerialException:
            #print("串口通信错误")
    
    def visualize(self, frame, target_pos, best_rect, error_x, error_y):
        """可视化处理结果"""
        height, width, _ = frame.shape
        center_x, center_y = width // 2, height // 2
        
        # 绘制图像中心
        cv2.circle(frame, (center_x, center_y), 10, (0, 255, 0), 2)
        cv2.line(frame, (center_x - 15, center_y), (center_x + 15, center_y), (0, 255, 0), 2)
        cv2.line(frame, (center_x, center_y - 15), (center_x, center_y + 15), (0, 255, 0), 2)
        


        # 绘制检测到的靶心
        if best_rect is not None:
            target_x, target_y = target_pos
            cv2.drawContours(frame, [best_rect], -1, (0,255,0), 2)
            #cv2.circle(frame, (target_x, target_y), target_radius, (0, 0, 255), 2)
            #cv2.circle(frame, (target_x, target_y), 5, (0, 0, 255), -1) #红色圆心
            cv2.circle(frame, (target_x, target_y), 5, (0, 0, 255), -1)
            
            # 绘制误差线
            cv2.line(frame, (center_x, center_y), (target_x, target_y), (255, 0, 0), 2)
        
        # 显示误差信息
        cv2.putText(frame, f"Error X: {error_x:.1f} px", (10, 30), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
        cv2.putText(frame, f"Error Y: {error_y:.1f} px", (10, 60), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
        
        # 显示系统状态
        status = "RUNNING" if self.running else "STOPPED"
        cv2.putText(frame, f"Status: {status}", (10, height - 30), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0) if self.running else (0, 0, 255), 2)
        
        return frame
    
    def start_targeting(self, duration=0):
        """启动瞄准系统"""
        if not self.calibrated:
            print("系统未标定，无法启动")
            return
        
        self.running = True
        start_time = time.time()
        
        print("瞄准系统启动...")
        
        while self.running:
            # 读取摄像头帧
            ret, frame = self.cap.read()
            if not ret:
                print("摄像头读取失败")
                break
            
            self.current_frame = frame.copy()
            
            # 检测靶心
            #target_pos, target_radius, mask = self.detect_target2(frame)
            best_rect,cen_x,cen_y = self.detect_target(frame)
            target_pos=(int(cen_x),int(cen_y))
            if self.target_found:
                # 计算误差
                error_x, error_y = self.calculate_error(target_pos)
                
                # PID控制计算
                control_x, control_y = self.pid_control(error_x, error_y)
                
                # 发送控制指令
                self.send_control_command(control_x, control_y)
            else:
                error_x, error_y = 0, 0
                print("未检测到靶心")
            
            # 可视化
            vis_frame = self.visualize(frame, target_pos, best_rect, error_x, error_y)
            cv2.imshow("Targeting System", vis_frame)
            #cv2.imshow("Mask", mask)
            
            # 检查时间限制
            if duration > 0 and (time.time() - start_time) > duration:
                print("达到时间限制，停止瞄准")
                self.running = False
            
            # 检查退出条件
            key = cv2.waitKey(1) & 0xFF
            if key == ord('q'):
                self.running = False
            
            # 检查误差是否在可接受范围内
            if abs(error_x) < 5 and abs(error_y) < 5 and self.target_found:
                print("目标锁定!")
                # 在实际应用中，这里可以触发激光发射
                self.laser_on = True
        
        self.stop_targeting()
    
    def stop_targeting(self):
        """停止瞄准系统"""
        self.running = False
        self.laser_on = False
        self.send_control_command(0, 0)  # 发送停止指令
        print("瞄准系统停止")
    
    def cleanup(self):
        """清理资源"""
        self.stop_targeting()
        self.cap.release()
        cv2.destroyAllWindows()
        #if self.ser.is_open:
            #self.ser.close()
        print("系统资源已释放")

class PIDController:
    """PID控制器实现"""
    def __init__(self, params):
        self.kp = params['kp']
        self.ki = params['ki']
        self.kd = params['kd']
        self.prev_error = 0
        self.integral = 0
        self.last_time = time.time()
    
    def update(self, error):
        """更新PID控制器并返回控制量"""
        current_time = time.time()
        dt = current_time - self.last_time
        self.last_time = current_time
        
        # 比例项
        p = self.kp * error
        
        # 积分项
        self.integral += error * dt
        i = self.ki * self.integral
        
        # 微分项
        derivative = (error - self.prev_error) / dt if dt > 0 else 0
        d = self.kd * derivative
        
        # 更新前次误差
        self.prev_error = error
        
        # 计算总控制量
        control = p + i + d
        
        # 限制控制量范围
        control = max(min(control, 100), -100)
        
        return control

def main():
    system = TargetingSystem()
    
    try:
        # 启动瞄准系统，持续运行直到手动停止
        system.start_targeting()
    except KeyboardInterrupt:
        print("程序被用户中断")
    finally:
        system.cleanup()

if __name__ == "__main__":
    main()